#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include "std_msgs/msg/float32_multi_array.hpp"
#include "tar_po/tar_po.h"
#include "tar_po/tar_po_initialize.h"

double vL;
double vR;

double PI=3.141593;

double tX=5.0;
double tY=0.0;

double dg,sig,dob,sio,v_o,w_o;//s_value

class DisAngCal: public rclcpp::Node
{
private:
    rclcpp::Time last_time_;
    // 声明一个订阅者esp32
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr command_subscribe_;
    // 声明话题发布者
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publisher_;
    //声明lidar_dis_angle主题
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr laser_scan_sub_;
    //声明imu主题
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    //
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        w_o=msg->angular_velocity.z;
    }
    void lidar_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        dob=msg->data[0];
        sio=msg->data[1];
    }
    void esp_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        // 创建消息
        RCLCPP_INFO(this->get_logger(), "得到esp32车轮线速度, 左轮vL=%f, 右轮vR=%f", msg->data[0], msg->data[1]);
        std_msgs::msg::Float32MultiArray s_value;
        s_value.data.resize(6);
        vL=static_cast<double>(msg->data[0]); // 使用类成员变量
        vR=static_cast<double>(msg->data[1]); // 使用类成员变量
        v_o=(vL+vR)/2.0;
        // 获取当前时间戳
        rclcpp::Time current_time = rclcpp::Clock().now();
        // 计算当前时间与上一次时间戳的差值
        rclcpp::Duration duration = current_time - last_time_;
        // 将时间间隔转换为秒
        double dt = duration.seconds();
        RCLCPP_INFO(this->get_logger(), "时间间隔dt=%f", dt);
        tar_po(tX,tY,vL,vR,dt,&dg,&sig);
        //导入至s_value
        s_value.data[0]=(float)dg;
        s_value.data[1]=(float)dob;
        s_value.data[2]=(float)sig;
        s_value.data[3]=(float)sio;
        s_value.data[4]=(float)v_o;
        s_value.data[5]=(float)w_o;
        RCLCPP_INFO(this->get_logger(), "得到神经网络变量: dg=%f, do=%f, sig=%f, sio=%f; 以及观测变量 vo=%f, wo=%f", dg, dob, sig, sio, v_o, w_o);
        command_publisher_->publish(s_value);
        // 更新上一次的时间戳为当前时间
        last_time_ = current_time;
        //
    }
public:
    // 构造函数,有一个参数为节点名称
    DisAngCal(std::string name):Node(name),last_time_(rclcpp::Clock().now())
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 订阅激光雷达lidar_dis_angle主题
        laser_scan_sub_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "lidar_dis_angle", 10, 
            std::bind(&DisAngCal::lidar_callback, this, 
            std::placeholders::_1));
        // 订阅imu主题
        imu_sub_=this->create_subscription<sensor_msgs::msg::Imu>(
            "imu/data_raw",10,std::bind(&DisAngCal::imu_callback,this,std::placeholders::_1));
        // 创建一个订阅者订阅esp32话题
        command_subscribe_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "espRE", 10, 
            std::bind(&DisAngCal::esp_callback, this, 
            std::placeholders::_1));
        // 创建发布者
        command_publisher_=this->create_publisher<std_msgs::msg::Float32MultiArray>("poRec", 10);
        // 初始化上一次时间戳为当前时间
        // last_time_ = rclcpp::Clock().now();
    }
};

int main(int argc, char **argv)
{
    static bool initialized = false;
    if (!initialized) {
        tar_po_initialize();
        initialized = true;
    }
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<DisAngCal>("dis_ang_cal");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}